//
// Created by geoffrey on 24-11-23.
//

#include "UUV.h"




auto sign = [](double x) { return (x >= 0) ? 1 : -1; };
// 微分方程：描述位置和速度的变化
Matrix<double, 6, 3> derivatives(double t , Matrix<double, 6, 2> state,const Matrix<double, 6, 1>& force)
    {
	// 水动力参数
	double Xu_=-4.874e-3,	Xwq=-88.246e-3,	Xvr=19.42e-3,	Xqq=3.43e-3,
	Xrr=-2.135e-3,	Xpp=0,			Xpr=-3.118e-3,	Xuu=-6.854e-3,
	Xww= 16.44e-3,	Xvv=6.652e-3,	Xvp=0,

	Yv_=-29.029e-3,	Ypq=46.866e-3,	Yv=-41.858e-3,		Yr=10.435e-3,
	Ywp=68.163e-3,	Yvv=-28.704e-3,	Yvr=-55.520e-3,
	Yr_=-0.396e-3,	Yp=-16.667e-3,	Yvq=0,				Yp_=0,
	Ywr=0,			Yvw=7.721e-3,	Yqr=-8.753e-3,
	Yp1p1=-510.681e-3,	Yr1r1=9.189e-3,	Yv1r1=-55.52e-3,	Yuu=0,
	Yv1v1=-28.704e-3,	Y1r1dr=0,

	Zw_=-126.6e-3,		Zw=-290.9e-3,	Zq=-145.5e-3,	Zvp=-31.9e-3,
	Zw1w1=-205.7e-3,	Zuu=0,
	Zq_=-1.4e-3,		Zpp=0,			Zpr=-0.396e-3,	Zrr=1.667e-3,
	Zvr=0,				Zvv=0,
	Zq1q1=-13.918e-3,	Zw1q1=-240.220e-3,
	Z1w1=-0.541e-3,  Zww=-40.69e-3,	Z1q1ds=-14.862e-3,


	Kp_=-0.5e-3,	Kqr=0.368e-3,	Kwr=2.044e-3,	Kvv=0,
	Kpp=-30e-3,		Kr_=0.0,		Kpq=2.223e-3,	Kv_=0.0,
	Kp=-1.547e-3,	Kr=-0.041e-3,	Kvq=-0.2044e-3, Kwp=5.858e-3,
	Kv=0,			Kvw=0,
	Kp1p1=-12.47e-3,Kr1r1=0,		Kuu=0,
	Kv1v1=0,		Kdr=0,

	Mq_=-5.043e-3,	Muw=-7.4e-3,	Mpr=5.2e-3,		Muq=-49.2e-3,
	Muu=0,			Mqq=-20.427e-3,
	Mpp=0,			Mrr=1.321e-3,	Mw_=-1.648e-3,	Mvp=-2.3e-3,
	Mvr=0,			Mvv=0,
	Mq1q1=-20.427e-3,	M1w1q=-54.422e-3,	Mu1w1=-1.854e-3,
	Mw1w1=-13.329e-3,
	Mww=2.621e-3,		M1q1ds=-7.807e-3,


	Nr_=-1.050e-3,	Nv=-2.872e-3,		Nr=-7.335e-3,	Nwp=-31.621e-3,
	Nrr=-6.453e-3,	Nvr=-13.998e-3,		Nvv=17.440e-3,
	Nv_=-0.396e-3,	Np=-8.908e-3,		Nvq=0,
	Np_=0,			Npq=-16.311e-3,		Nqr=0.417e-3,
	Nwr=0,			Nvw=4.148e-3,
	Np1p1=13.253e-3,Nr1r1=-6.453e-3,	N1v1r=-13.998e-3,
	Nuu=0,			Nv1v1=17.440e-3,	N1r1dr=0,

	Xdsds=-3.370e-3,
	Xdrdr=-7.578e-3,

	Ydr=14.409e-3,

	Zds=-14.114e-3,

	Mds=-9.232e-3,

	Ndr=2*9.777e-3;


	Matrix<double, 6, 1> dx;
	Matrix<double, 6, 1> dv;
	Matrix<double, 6, 3> dstate;
	Matrix<double, 6, 1> x_b_1 = state.block(0, 1, 6, 1);
	Matrix<double, 6, 1> x_a = state.block(0, 0, 6, 1);
	Matrix<double, 6, 6> MRB;
	Matrix<double, 6, 6> CRB;
	Matrix<double, 6, 6> MA;

	Matrix<double, 6, 6> CA;
	Matrix<double, 6, 1> D;
	Matrix<double, 6, 1> g;
	Matrix<double, 6, 1> g0;
	Vector3d fgb;	//载体坐标系重力
	Vector3d fbb;	//载体坐标系浮力
	Vector3d fgn;	//大地坐标系重力
	Vector3d fbn;	//大地坐标系浮力


	double m = 20;//5.7831e3;					//质量
	double L = 2;							//长度
	double rho = 1025;						//海水密度
	Vector3d rg;							//质心坐标
	rg << 0.0, 0.0, 31e-3;
	Vector3d rb;							//浮心坐标
	rg << 0.0, 0.0, -31e-3;
	Matrix<double, 3, 3> I;					//转动惯量
	I << 1e100, 0, 0, 0, 20, 0 ,0 ,0 ,15;			//转动惯量初始化
	//I << 1e100, 0, 0, 0, 1.05e4, 0 ,0 ,0 ,1.2e4;			//转动惯量初始化



	Matrix<double, 3, 3> I3;				//3X3单位阵
	I3 << MatrixXd::Identity(3, 3);
	Matrix<double, 3, 1> v1 = x_b_1.block(0, 0, 3, 1);
	Matrix<double, 3, 1> v2 = x_b_1.block(3, 0, 3, 1);
	Matrix<double, 3, 1> &wb = v2;
	MRB << m * I3 , m * S(rg).transpose() , m * S(rg) , I ;
	CRB << m * S(wb) , -m * S(wb) * S(rg) , m * S(rg) * S(wb) , -S(I*wb);


	Vector3d eulerAngle = x_a.block(3, 0, 3, 1);
	//欧拉角转旋转向量
	AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
	AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
	AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
	//旋转向量转四元数
	Quaterniond quaternion;
	quaternion=yawAngle*pitchAngle*rollAngle;
	MA <<	-rho/2*pow(L,3)*Xu_,0,0,0,0,0,
		0,-rho/2*pow(L,3)*Yv_,0,0,0,-rho/2*pow(L,4)*Yr_,
		0,0,-rho/2*pow(L,3)*Zw_,0,-rho/2*pow(L,4)*Zq_ ,0,
		0,0,0,1e100,0,0,
		0,0, -rho/2*pow(L,4)*Mw_,0,-rho/2*pow(L,5)*Mq_,0,
		0,-rho/2*pow(L,4)*Nv_ ,0,0,0,-rho/2*pow(L,5)*Nr_;
	Matrix<double, 3, 3> MA11 = MA.block(0, 0, 3, 3);
	Matrix<double, 3, 3> MA12 = MA.block(0, 3, 3, 3);
	Matrix<double, 3, 3> MA21 = MA.block(3, 0, 3, 3);
	Matrix<double, 3, 3> MA22 = MA.block(3, 3, 3, 3);
	CA <<	MatrixXd::Zero(3, 3) , -S(MA11 * v1 + MA12 * v2),
			-S(MA11 * v1 + MA12 * v2)    , -S(MA21 * v1 + MA22 * v2);

	Matrix3d R (quaternion);



	fgn << 0 , 0 , 9.81 * m ;
	fbn << 0 , 0 , -9.81 * m;
	fgb = R.transpose() * fgn;
	fbb = R.transpose() * fbn;

	g << fgb + fbb , rg.cross(fgb) + rb.cross(fbb);
	g = -g;
	g0 << 0 , 0 , 0 , 0 , 0 , 0 ;

	Matrix<double, 3, 1> vc ;
	vc << 0 , 0 , 0 ;
	double	u = x_b_1(0), v = x_b_1(1), w = x_b_1(2), p = x_b_1(3), q = x_b_1(4), r = x_b_1(5);
	double 	phi = x_a(3), theta = x_a(4), psi = x_a(5);
	double 	x_1 = vc(0) + u * cos(psi) * cos(theta) + v * (cos(psi) * sin(theta) * sin(phi) - sin(psi) * cos(phi)) + w * (sin(psi) * sin(phi) + cos(psi) * cos(phi) * sin(theta)),
			y_1 = vc(1) + u * sin(psi) * cos(theta) + v * (cos(psi) * cos(phi) + sin(psi) * sin(theta) * sin(phi)) + w * (sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi)),
			z_1 = vc(2) + -u * sin(theta) + v * cos(theta) * sin(phi) + w * cos(theta) * cos(phi),
			phi_1 = p + q * sin(phi) * tan(theta) + r * cos(phi) * tan(theta),
			theta_1 = q * cos(phi) - r * sin(phi),
			psi_1 = q * sin(phi) / cos(theta) + r * cos(phi) / cos(theta);

	dx << x_1 , y_1 , z_1 , phi_1 , theta_1 , psi_1;

	D <<   rho/2*pow(L,4)*(Xqq*q*q+Xrr*r*r)+
		   rho/2*pow(L,3)*(Xwq*w*q+Xvr*v*r)+
		   rho/2*pow(L,2)*(Xvv*(pow(v,2))+Xww*(pow(w,2))+Xuu*(u)*abs(u)),

		   rho/2*pow(L,4)*(Yqr*q*r+Yr1r1*r*abs(r))+
		   rho/2*pow(L,3)*(Yr*u*r+Yvq*v*q+Ywr*w*r+Yv1r1*sign(v)*sqrt(pow(v,2)+pow(w,2))*abs(r))+
		   rho/2*pow(L,2)*(Yuu*(pow(u,2))+Yv*u*v+Yvw*v*w+Yv1v1*v*sqrt(pow(v,2)+pow(w,2))),

		   rho/2*pow(L,4)*(Zrr*r*r+Zq1q1*q*abs(q))+
		   rho/2*pow(L,3)*(Zq*u*q+Zvr*v*r+Zw1q1*sign(w)*sqrt(pow(v,2)+pow(w,2))*abs(q))+
		   rho/2*pow(L,2)*(Zuu*(pow(u,2))+Zw*u*w+Z1w1*u*abs(w)+Zvv*(pow(v,2))+Zww*abs(w)*sqrt(pow(v,2)+pow(w,2))+Zw1w1*(w)*sqrt(pow(v,2)+pow(w,2))),

		   0,

		   -rho/2*pow(L,5)*(Mq1q1*q*abs(q)+Mrr*r*r)+
		   rho/2*pow(L,4)*(Muq*u*q+Mvr*v*r+M1w1q*sqrt(pow(v,2)+pow(w,2))*q)+
		   rho/2*pow(L,3)*(Muu*(pow(u,2))+Mvv*(pow(v,2))+Muw*u*w+Mu1w1*u*abs(w)+Mw1w1*w*sqrt(pow(v,2)+pow(w,2))+Mww*abs(w)*sqrt(pow(v,2)+pow(w,2))),


		   -rho/2*pow(L,5)*(Nr1r1*r*abs(r)+Nqr*q*r)+
		   rho/2*pow(L,4)*(Nr*u*r+Nwr*w*r+Nvq*v*q+N1v1r*sqrt(pow(v,2)+pow(w,2))*r)+
		   rho/2*pow(L,3)*(Nuu*(pow(u,2))+Nv*u*v+Nvw*v*w+Nv1v1*(v)*sqrt(pow(v,2)+pow(w,2)));

	D = -D;
	Matrix<double ,6 ,1> f =  -(CRB + CA)*x_b_1 -  D;
/*
	//论文模型
	Matrix<double ,6 ,6> M_sb;
	double mu=25,mv=17.5,mw=30,mq=22.5,mr=15;
	M_sb<<	25,0,0,0,0,0,
			0,17.5,0,0,0,0,
			0,0,30,0,0,0,
			0,0,0,1e100,0,0,
			0,0,0,0,22.5,0,
			0,0,0,0,0,15;
	Matrix<double ,6 ,6> D_sb;
	D_sb<<30,0,0,0,0,0,
			0,30,0,0,0,0,
			0,0,20,0,0,0,
			0,0,0,0,0,0,
			0,0,0,0,20,0,
			0,0,0,0,0,20;
	f << mv*v*r-mw*w*q,-mu*u*r,mu*u*q,0,(mw-mu)*u*w,(mu-mv)*u*v;
	f = f - D_sb*x_b_1;
	dv = M_sb.inverse()*(force + f - g - g0);*/
	dv = (MRB + MA).inverse()*(force - (CRB + CA)*x_b_1 -  D - g - g0);
//cout<<"dv = "<<dv<<endl;


	dstate << dx , dv , f;
	return	dstate;
    }


UUV::UUV()
    :state(MatrixXd::Zero(6,2)) {
	State.push_back(MatrixXd::Zero(6,2));
	X_Isometry.push_back(Eular2Isometry(state.block(0, 0, 6, 1)));
};
UUV::UUV(const Matrix<double, 6, 2> &_state)
	:state(_state) {
	State.push_back(_state);
	X_Isometry.push_back(Eular2Isometry(state.block(0, 0, 6, 1)));
};

void UUV::update(Matrix<double, 6, 1> force)
    {
	//扰动
	for (int i = 0; i < 6; i++) {
		force(i,0) = force(i,0) + 5 * sin(0.1 * t);
	}
	f = derivatives(t, state,force).block(0,2,6,1);
	// Runge-Kutta 四阶法迭代
	// 计算四阶Runge-Kutta中的四个中间值
	Matrix<double, 6, 2> k1 = derivatives(t, state,force).block(0,0,6,2);
	Matrix<double, 6, 2> k2 = derivatives(t + h / 2, state + h * k1 / 2, force).block(0,0,6,2);
	Matrix<double, 6, 2> k3 = derivatives(t + h / 2, state + h * k2 / 2, force).block(0,0,6,2);
	Matrix<double, 6, 2> k4 = derivatives(t + h, state + h * k3, force).block(0,0,6,2);

    // 更新

    state = state + h * (k1 + 2 * k2 + 2 * k3 + k4) / 6.0;
	++i;
	t = i * h;
	State.push_back(state);
	X_Isometry.push_back(Eular2Isometry(state.block(0, 0, 6, 1)));
    }

double UUV::getT() const {
	return t;
}

vector<Matrix<double, 6, 2>,aligned_allocator<Matrix<double, 6, 2>>> UUV::getState() const {
	return State;
}

Matrix<double, 6, 2> UUV::getstate() const {
	return state;
}

Matrix<double, 6, 1> UUV::getF() const {
	return f;
}



